package org.scalpel.locodo;

/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * The class OdometrySingleSingle helps to compute current position 
 * of one motor. It needs physical parameters of the wheel, 
 * such as encoder wheel perimeter...
 * It also helps to compute relative position of a target.
 */
public class OdometrySingle {

  private int fEnc_prev;                        // Last encoder values
  private volatile double fEncoderWheelPerimeter;// encoderWheelPerimeter Perimeter of the encoder wheel
  private volatile int fEncoderTickPerRotation; // encoderTickPerRotation Encoder precision
  private double fCurrPosition;                  // current position of the wheel
	private double fEncoderWheelDiameter;						// encoder wheel diameter


  /**
   * default constructor.. 
   * @param encoderWheelDiameter Diameter of the encoder wheel
   * @param encoderTickPerRotation Encoder precision
   */
  public OdometrySingle( double encoderWheelDiameter, int encoderTickPerRotation){

	  fEncoderWheelDiameter = encoderWheelDiameter;
	  fEncoderWheelPerimeter = Math.PI * fEncoderWheelDiameter;
    fEncoderTickPerRotation = encoderTickPerRotation;
    fEnc_prev = 0;
    fCurrPosition = 0.f;

  }
  

  /**
   * setPosition : set the current position of the robot
   * @param position Position
   */
  public void setPosition( double position ){ 
    fCurrPosition = position; 
  }
  

  /**
   * getCurrentPosition : get the current position motor
   * @return Current position.
   */
  public double getPosition(){ 
    return fCurrPosition; 
  }


  /**
   * update position according to the wheel geometry and current encoder value
   * need frequent update to keep high precision
   * @param currEncoderValue encoder value
   * @return return current position
   */
   public double updatePosition(int currEncoderValue){

     // compute the difference between the last and current encoder's position
     double deltaMeter = ((double)(currEncoderValue - fEnc_prev)
                               / (double)fEncoderTickPerRotation
                             ) * fEncoderWheelPerimeter;

     // Save the new encoder value
     fEnc_prev =  currEncoderValue;

     // Update current position
     fCurrPosition += deltaMeter;

     return fCurrPosition;
   };


  /**
   * update position according to the wheel geometry and current encoder value
   * @param target position of a target
   * @return relative position of the target
   */
  public double getRelativeTargetPosition( double target ){

    // compute target position in the robot space
    return getRelativeTargetFromPosition( fCurrPosition, target );

  };


  /**
   * get a relative target position
   * @param position
   * @param target
   * @return relative position of the target in the given position reference
   */
  double getRelativeTargetFromPosition( double position, double target )
  {
    return target - position;
  };
}
